"""
机器人通信模块
负责与机器人进行通信，发送动作指令
"""

import time
import threading
import queue
from typing import Dict, Any, Callable
from enum import Enum
import json
import socket

try:
    import serial
    SERIAL_AVAILABLE = True
except ImportError:
    SERIAL_AVAILABLE = False


class CommunicationState(Enum):
    """通信状态枚举"""
    DISCONNECTED = "disconnected"  # 未连接
    CONNECTING = "connecting"      # 连接中
    CONNECTED = "connected"        # 已连接
    ERROR = "error"                # 错误


class RobotCommunicator:
    """
    机器人通信器
    负责与机器人进行通信
    """
    
    def __init__(self):
        """初始化机器人通信器"""
        self.state = CommunicationState.DISCONNECTED
        self.robot_address = ""
        self.connection = None
        self.command_queue = queue.Queue()
        self.response_callbacks = {}
        self.comm_thread = None
        self.running = False
        self.sequence_buffer = []  # 动作序列缓冲区

    def connect(self, address: str) -> bool:
        """
        连接到机器人
        
        Args:
            address: 机器人地址，可以是IP:端口格式(TCP)或串口名称
            
        Returns:
            bool: 是否连接成功
        """
        # 断开现有连接
        if self.state == CommunicationState.CONNECTED:
            self.disconnect()
            
        self.robot_address = address
        self.state = CommunicationState.CONNECTING
        
        try:
            print(f"正在连接到机器人: {address}")
            
            # 判断连接类型
            if ':' in address:
                # TCP连接 (WiFi)
                host, port = address.split(':')
                port = int(port)
                
                # 创建TCP socket连接
                self.connection = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                self.connection.settimeout(5.0)  # 恢复原来的5秒超时时间
                self.connection.connect((host, port))
            else:
                # 串口连接
                return self._connect_serial(address)
            
            self.state = CommunicationState.CONNECTED
            
            # 启动通信线程
            self._start_communication_thread()
            
            print(f"成功连接到机器人: {address}")
            return True
            
        except socket.timeout:
            print(f"连接机器人超时: {address}")
            self.state = CommunicationState.ERROR
            return False
        except ConnectionRefusedError:
            print(f"连接被拒绝，请检查机器人是否正在运行并监听: {address}")
            self.state = CommunicationState.ERROR
            return False
        except Exception as e:
            print(f"连接机器人失败: {e}")
            self.state = CommunicationState.ERROR
            return False
            
    def disconnect(self):
        """断开与机器人的连接"""
        self.running = False
        if self.comm_thread and self.comm_thread.is_alive():
            self.comm_thread.join(timeout=2)
            
        # 关闭连接
        if self.connection:
            if isinstance(self.connection, socket.socket):
                self.connection.close()
            elif SERIAL_AVAILABLE and isinstance(self.connection, serial.Serial):
                self.connection.close()
            self.connection = None
            
        self.state = CommunicationState.DISCONNECTED
        print("已断开与机器人的连接")
        
    def _start_communication_thread(self):
        """启动通信线程"""
        self.running = True
        self.comm_thread = threading.Thread(target=self._communication_loop)
        self.comm_thread.daemon = True
        self.comm_thread.start()
        
    def _communication_loop(self):
        """通信循环"""
        while self.running:
            try:
                # 发送队列中的命令
                if not self.command_queue.empty():
                    command = self.command_queue.get_nowait()
                    self._send_command(command)
                    
                # 短暂休眠避免过度占用CPU
                time.sleep(0.01)
                
            except queue.Empty:
                # 队列为空，继续循环
                continue
            except Exception as e:
                print(f"通信循环异常: {e}")
                self.state = CommunicationState.ERROR
                
    def _send_command(self, command: Dict[str, Any]):
        """
        发送命令到机器人
        
        Args:
            command: 命令字典
        """
        if self.state != CommunicationState.CONNECTED:
            print("机器人未连接，无法发送命令")
            return
            
        try:
            # 构造命令字符串
            command_str = json.dumps(command, ensure_ascii=False) + "\n"
            
            # 发送命令
            if isinstance(self.connection, socket.socket):
                # TCP连接发送
                self.connection.send(command_str.encode('utf-8'))
            elif SERIAL_AVAILABLE and isinstance(self.connection, serial.Serial):
                # 串口连接发送
                self.connection.write(command_str.encode('utf-8'))
                self.connection.flush()
            else:
                # 串口连接发送（模拟）
                print(f"串口发送命令: {command_str.strip()}")
            
            print(f"发送命令到机器人: {command_str.strip()}")
            
        except socket.timeout:
            print("发送命令超时")
            self.state = CommunicationState.ERROR
        except ConnectionResetError:
            print("连接被重置，机器人可能已断开连接")
            self.state = CommunicationState.ERROR
        except BrokenPipeError:
            print("管道破裂，机器人连接可能已断开")
            self.state = CommunicationState.ERROR
        except serial.SerialTimeoutException:
            print("串口发送超时")
            self.state = CommunicationState.ERROR
        except Exception as e:
            print(f"发送命令失败: {e}")
            self.state = CommunicationState.ERROR
            
    def execute_action(self, action):
        """
        执行单个动作
        
        Args:
            action: 动作对象
        """
        if self.state != CommunicationState.CONNECTED:
            # 机器人未连接时，仅打印日志而不阻止动作执行
            print(f"机器人未连接，动作已生成但未发送: {getattr(action, 'action_id', 'unknown')}")
            # 仍然打印动作信息以便调试
            print(f"  关节角度: {getattr(action, 'joint_angles', {})}")
            print(f"  持续时间: {getattr(action, 'duration', 1.0)}s")
            return
            
        command = {
            'type': 'move_joints',
            'timestamp': getattr(action, 'timestamp', time.time()),
            'joints': getattr(action, 'joint_angles', {}),
            'duration': getattr(action, 'duration', 1.0)
        }
        
        self.command_queue.put(command)
        
    def send_text_message(self, text: str):
        """
        发送文本消息到机器人
        
        Args:
            text: 要发送的文本消息
        """
        if self.state != CommunicationState.CONNECTED:
            # 机器人未连接时，仅打印日志而不阻止消息发送
            print(f"机器人未连接，文本消息未发送: {text}")
            return
            
        command = {
            'type': 'text_message',
            'timestamp': time.time(),
            'text': text
        }
        
        self.command_queue.put(command)

    def execute_sequence(self, sequence):
        """
        执行动作序列
        
        Args:
            sequence: 动作序列
        """
        if self.state != CommunicationState.CONNECTED:
            # 机器人未连接时，仅打印日志而不阻止序列执行
            print("机器人未连接，序列已生成但未发送")
            if hasattr(sequence, 'actions'):
                print(f"  序列包含 {len(sequence.actions)} 个动作")
                for i, action in enumerate(sequence.actions):
                    print(f"    动作 {i+1}: {getattr(action, 'action_id', 'unknown')}")
            return
            
        # 将序列添加到缓冲区
        if hasattr(sequence, 'actions'):
            self.sequence_buffer.extend(sequence.actions)
        else:
            print("序列对象格式不正确")
        
    def send_config(self, config: Dict[str, Any]):
        """
        发送配置到机器人
        
        Args:
            config: 配置字典
        """
        command = {
            'type': 'config',
            'data': config
        }
        
        self.command_queue.put(command)
        
    def send_wifi_config(self, wifi_config: Dict[str, Any]):
        """
        通过串口发送WiFi配置到ESP32
        
        Args:
            wifi_config: WiFi配置字典，包含ssid, password等
        """
        command = {
            'type': 'wifi_config',
            'data': wifi_config
        }
        
        # 直接发送WiFi配置命令
        self._send_command(command)
        
    def send_motion_control(self, yaw_speed: float, speed: float, leg_length: float):
        """
        发送运动控制指令 (0xAA)
        数据包: [0xAA, yaw_speed, speed, leg_length]
        
        Args:
            yaw_speed: 转向速度 (-1.5~1.5 rad/s)
            speed: 前进速度 (-1.0~1.0 m/s)
            leg_length: 腿长 (0.07~0.09m)
        """
        # 将实际值映射到控制值
        yaw_control = int(((-yaw_speed / 1.5) + 1) * 100)  # 映射到 0-200 范围
        speed_control = int(((speed / 1.0) + 1) * 100)    # 映射到 0-200 范围
        leg_control = int(((leg_length - 0.07) / 0.02) * 200)  # 映射到 0-200 范围
        
        # 限制范围
        yaw_control = max(0, min(200, yaw_control))
        speed_control = max(0, min(200, speed_control))
        leg_control = max(0, min(200, leg_control))
        
        # 构造uint8_t数据包
        data = [0xAA, yaw_control, speed_control, leg_control]
        
        # 发送数据
        self._send_uint8_array(data)
        
    def send_stand_up_command(self):
        """
        发送起立控制指令 (0xAB)
        数据包: [0xAB]
        """
        data = [0xAB]
        self._send_uint8_array(data)
        
    def send_data_sample_command(self, duration: int = 0):
        """
        发送数据采样指令 (0xAC)
        数据包: [0xAC, duration]
        
        Args:
            duration: 采样持续时间(秒)，0表示无限期
        """
        # 限制duration范围
        duration = max(0, min(255, duration))
        data = [0xAC, duration]
        self._send_uint8_array(data)
        
    def send_debug_command(self, debug_data: list):
        """
        发送调试指令 (0xDB)
        数据包: [0xDB, ...]
        
        Args:
            debug_data: 调试数据列表
        """
        if not isinstance(debug_data, list):
            raise ValueError("debug_data must be a list")
            
        data = [0xDB] + debug_data
        self._send_uint8_array(data)
        
    def _send_uint8_array(self, data: list):
        """
        发送uint8_t数组到机器人
        
        Args:
            data: uint8_t数据列表
        """
        if self.state != CommunicationState.CONNECTED:
            print("错误: 机器人未连接")
            return
            
        try:
            # 确保所有数据都是uint8_t范围内的值
            uint8_data = [max(0, min(255, int(x))) for x in data]
            
            # 转换为字节数据
            byte_data = bytes(uint8_data)
            
            # 发送数据
            if isinstance(self.connection, serial.Serial):
                self.connection.write(byte_data)
                self.connection.flush()
                print(f"发送uint8_t数据: {uint8_data}")
            else:
                # 如果是TCP连接，可能需要其他处理方式
                print("警告: 当前连接类型不支持直接发送uint8_t数组")
                
        except Exception as e:
            print(f"发送uint8_t数组时出错: {e}")
            self.state = CommunicationState.ERROR
            
    def _connect_serial(self, port: str) -> bool:
        """
        连接到ESP32串口
        
        Args:
            port: 串口名称
            
        Returns:
            bool: 是否连接成功
        """
        if not SERIAL_AVAILABLE:
            print("串口功能不可用，请安装pyserial库")
            return False
            
        self.state = CommunicationState.CONNECTING
        
        try:
            # 解析串口名称，提取实际的COM端口
            # 处理类似 "COM1 - ELTIMA Virtual Serial Port (COM1->COM2)" 的格式
            actual_port = port.split()[0] if port and ' ' in port else port
            # 确保端口名称以COM开头（在Windows上）
            if not actual_port.startswith('COM'):
                # 如果不是以COM开头，尝试查找COM部分
                import re
                com_match = re.search(r'COM\d+', port)
                if com_match:
                    actual_port = com_match.group(0)
                else:
                    actual_port = port  # 回退到原始端口
            
            print(f"正在连接到ESP32串口: {actual_port} (原端口: {port})")
            
            # 创建串口连接
            self.connection = serial.Serial(
                port=actual_port,
                baudrate=115200,
                timeout=2,
                write_timeout=2
            )
            
            self.state = CommunicationState.CONNECTED
            print(f"成功连接到ESP32串口: {actual_port}")
            return True
            
        except serial.SerialException as e:
            print(f"串口连接失败: {e}")
            self.state = CommunicationState.ERROR
            return False
        except Exception as e:
            print(f"连接串口时发生未知错误: {e}")
            self.state = CommunicationState.ERROR
            return False

    def get_status(self) -> Dict[str, Any]:
        """
        获取机器人状态
        
        Returns:
            Dict[str, Any]: 状态信息
        """
        return {
            'state': self.state.value,
            'address': self.robot_address,
            'queue_size': self.command_queue.qsize()
        }


# 全局机器人通信器实例
robot_communicator = RobotCommunicator()


def get_robot_communicator() -> RobotCommunicator:
    """
    获取全局机器人通信器实例
    
    Returns:
        RobotCommunicator: 机器人通信器实例
    """
    return robot_communicator